#ifndef LB_QUATERNIONS_H_
#define LB_QUATERNIONS_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdlib.h>
#include <Math/LB_Math.h>
#include<string.h>
#define SAFE_SQRT(x) (x>=0?sqrt(x):0)

typedef struct Quaternions{

double v[4];

}Quaternions;


static inline void quaternions_init(Quaternions* q )
{
    memset(q->v,0,sizeof(double)*4);
}


static inline Quaternions quaternions_mult(Quaternions q1,Quaternions q2)
{
    Quaternions re;
    quaternions_init(&re);

    re.v[0]=q1.v[0]*q2.v[0]-q1.v[1]*q2.v[1]- q1.v[2]*q2.v[2]- q1.v[3]*q2.v[3];

    re.v[1]=q1.v[0]*q2.v[1]+ q1.v[1]*q2.v[0] + q1.v[2]*q2.v[3]- q1.v[3]*q2.v[2];

    re.v[2]=q1.v[0]*q2.v[2]+q1.v[2]*q2.v[0]+ q1.v[3]*q2.v[1]- q1.v[1]*q2.v[3];

    re.v[3]=q1.v[0]*q2.v[3]+q1.v[3]*q2.v[0]+ q1.v[1]*q2.v[2]- q1.v[2]*q2.v[1];

    return re;
}



Quaternions q_rota(Quaternions q1,double* x); 

Quaternions quaternions_plus(Quaternions q1,Quaternions q2);




static inline Quaternions quaternions_conjugate(Quaternions q )
{
    Quaternions re;
    quaternions_init(&re);
    re.v[0]=q.v[0];
    re.v[1]=-q.v[1];re.v[2]=-q.v[2];re.v[3]=-q.v[3];
    return re;
}

static inline double quaternions_norm(Quaternions q )
{
    return SAFE_SQRT(q.v[0]*q.v[0]+q.v[1]*q.v[1]+q.v[2]*q.v[2]+q.v[3]*q.v[3]);
}




static inline Quaternions quaternions_inverse(Quaternions q )
{
    double norm= q.v[0]*q.v[0]+q.v[1]*q.v[1]+q.v[2]*q.v[2]+q.v[3]*q.v[3];

    Quaternions re;
    quaternions_init(&re);

    re.v[0]=q.v[0]/norm;
    re.v[1]=-q.v[1]/norm;re.v[2]=-q.v[2]/norm;re.v[3]=-q.v[3]/norm;

    return re;
}


static inline double* get_matrix_from_rotation(double theta,double* axis)
{
    normalize(axis,3); 
    double *re=(double*)malloc(sizeof(double)*9);
    double sint=sin(theta);
    double cost=cos(theta);
    re[0]=cost+(1-cost)*axis[0]*axis[0];re[1]=(1-cost)*axis[0]*axis[1]-axis[2]*sint;re[2]=(1-cost)*axis[0]*axis[2]+axis[1]*sint;
    
    re[3]=sint*axis[2]+(1-cost)*axis[0]*axis[1];re[4]=cost+(1-cost)*axis[1]*axis[1];re[5]=(1-cost)*axis[1]*axis[2]-axis[0]*sint;

    re[6]=(1-cost)*axis[0]*axis[2]-axis[1]*sint;re[7]=(1-cost)*axis[2]*axis[1]+axis[0]*sint;re[8]=(1-cost)*axis[2]*axis[2]+cost;
    
    return re;
}

double* get_rotation_from_matrix(double* matrix);




double*  get_rotation_from_quaternions( Quaternions q  );

double*  get_matrix_from_quaternions( Quaternions q  );


static inline Quaternions quaternions_power(Quaternions q,double t)
{
    Quaternions re;
    quaternions_init(&re);
    double norm= quaternions_norm(q);
    double theta=acos(q.v[0]/norm);
    
    double a=pow(norm,t);
    re.v[0]=a*cos(theta*t);
    re.v[1]=a*sin(theta*t)*(q.v[1]/(norm*sin(theta)));
    re.v[2]=a*sin(theta*t)*(q.v[2]/(norm*sin(theta)));
    re.v[3]=a*sin(theta*t)*(q.v[3]/(norm*sin(theta)));

    return re;
}

static inline Quaternions q_rotas_interpolation(Quaternions q1,Quaternions q2,double t)
{
    return quaternions_mult(quaternions_power(q1,1-t),quaternions_power(q2,t));
}


// Quaternions get_quaternions_from_rotation(double theta,double *axis)
// {
// }



#ifdef __cplusplus
}
#endif

#endif 